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In order for a team of several Automated Underwater Vehicles (AUVs), such as 
the ARIES, to operate cooperatively, operators require a cost effective position 
estimation method. Range only measurement (ROM) position estimation provides this 
and a means for the AUVs to identify each other’s position. Position estimation usually 
requires at least two range measurements from known points to solve for a vessel’s 
position. However, under certain conditions, one range only measurement can provide a 
simpler solution. This thesis proves ROM as a viable means of target tracking and 
position estimation. Determining the accuracy and observability of ROM serve as the 
primary focus. The ROM model setup and execution are discussed with specific 
attention given to the details of the Extended Kalman Filter (EKF) and calculations 
required to determine the system’s observability. 
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I. 



INTRODUCTION 



A. BACKGROUND 

The number of Third World/non-super power countries exercising their armed 
forces is increasing as border disputes, religious difference, and etc. fuel aggression. 
Although a majority of these nations have no ill intentions towards the United States, 
they may pose a very dangerous threat to U.S. vessels. By using minefields, a nation can 
stifle the use of a waterway. During the Persian Gulf War, mine fields were laid in the 
Persian Gulf by Iraqi forces. As a result of the mine layings, sea-lanes were severely 
restricted until proper mine clearing of frequented water ways could be completed. To 
clear minefields the Navy currently uses specialized ships, aircraft, and personnel. In any 
of the above methods, there is a very high risk of human life loss in the event of an 
accident. As a result, alternate means of conducting mine clearing tasks have been 
explored including the use of marine mammals and Autonomous Underwater Vehicles 
(AUVs) /Unmanned Underwater Vehicles (UUVs). Marine mammal systems have the 
drawbacks of limited numbers, and extensive, expensive, training requirements. 
Additionally, their use would likely cause opposition from the public. Fortunately, 
AUVs are designed to be low maintenance, low cost, ‘expendable’ vehicles designed to 
carry out the mission in its program. 

AUVs and UUVs are rapidly finding increased potential in military applications. 
From deep-water salvage to mine hunting/clearing operations, this new breed of vehicles 
provides a means for dangerous missions to be carried out without endangering 
personnel. More specifically, use of low cost AUVs in mine hunting/clearing operations 
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appears specifically attractive since there is no risk of human life loss or damaging 
expensive ships. 

It is proposed that teams of AUVs working together may be used to search and 
identify large minefields. Such a team would consist of several “worker” vehicles and 
one “master” vehicle. The “worker” vehicles are expendable, low cost, AUVs designed 
to locate and possibly detonate enemy mines. The “master” vehicles are better equipped 
with more complex navigation, sonar, and communication suites. Pre-programmed 
“workers” would carry out their specific missions while the “master” vehicle would track 
and direct the drones to their specific waypoints. For a “master” vehicle to track several 
“workers”, problems arise for a small, weight-conscious, AUV. Conventional position 
estimation in underwater applications usually consists of either active or passive towed 
array sonar. Unfortunately each application has drawbacks that preclude their use 
onboard the AUV. Active sonar would provide the most up to date and accurate 
positions of the worker vehicles. However, the transducer required to provide accurate 
positions of all the workers would be too large, require too much power, and be cost 
prohibitive for the size required in an AUV. Passive sonar is not a good choice for the 
AUV due to the time-late properties of its information. Additionally, vehicle mounted 
listening equipment that is of acceptable size, quality, and range is cost prohibitive, and 
use of a passive array requires handling equipment, which is not conducive to use in an 
AUV. Both passive and active tracking methods lack a means to explicitly discern mine- 
like objects from friendly AUVs. 

In an effort to conserve space and weight, a means of Range only Measurement 
(ROM) position estimation is proposed. Song (1999) has demonstrated the observability 
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of ROM target tracking through use of simulations. Use of ROM tracking combines 
aspects of conventional active and passive tracking techniques. Like an active sensor, 
sound signals are emitted into the water using space and weight conscious acoustic 
modems transmit data through the water among vehicles in the team. Each signal 
contains “sent time”, course, and identification information. This information enables the 
“master” vehicle to estimate the distance between sender and receiver, and to identify 
each sender. Upon receiving the signal from the “master” vehicle, the drone vehicle 
transmits its signal to “master” vehicle. When the “master” vehicle receives this signal, 
the distance between the two vehicles can be estimated by calculating time of travel for 
the signal. To ensure maximum accuracy, the “master” vehicle would use water property 
values attained from ‘now’ data. 

B. SCOPE OF TfflS WORK 

The overall problem of ROM position estimation is complex and diverse since it 
is usual that either range and bearing, or two range values are needed for position 
estimation. This study includes the development of the dynamic model and an Extended 
Kalman Filter (EKF) to demonstrate the accuracy and viability of ROM techniques for 
position estimation. The purpose of this thesis is twofold: 

1. To prove the observability of ROM tracking techniques. 

2. To illustrate the accuracy of ROM tracking by comparing it to dead reckoning, 
a common means of simple navigation. 

Chapter II will provide the background of the Naval Postgraduate School (NPS) 
Acoustic Reconnaissance Interactive Exploratory Server (ARIES) AUV including 
navigation and sensor capabilities that are the basis of the “master” AUV’s capabilities. 
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Chapter III explains the theory and modeling of the simulation with detailed descriptions 
of the construction of the Extended Kalman Filter (EKF), and the means of determining 
the system’s observability. Chapter IV summarizes the results of the simulations. 
Chapter V presents the conclusions gathered from the results, and offers 
recommendations for continued research on this topic. 
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II. OVERVIEW OF ARIES 



A brief overview of the Naval Postgraduate School ARIES AUV shall provide 
some the working knowledge necessary to understand why certain assumptions were 
made, and to familiarize the reader with the AUV. 

ARIES weighs 225 kilograms and measures about three m long, 0.4 m wide and 
0.25 m high. The hull is constructed of one-quarter inch thick aluminum and forms the 
main pressure vessel that houses all electronics, computers, and batteries. External 
sensors are housed in the flooded fiberglass nose. Figure 2. 1 shows the ARIES 
component layout. The vehicle has a top speed of three and one-half knots, which it may 
maintain for approximately four hours. The ARIES was primarily designed for shallow 
water operations and can operate safely down to 30 meters (Marco, Healey, 2000). 

A. NAVIGATION 

The sensor suite used for navigation includes a RD Instruments Navigator 
Doppler Velocity Log (DVL) that also contains a magnetic compass. This instrument 
measures the vehicle ground speed, altitude, and magnetic heading. Angular rates and 
accelerations are measured using a 3-axis Motion Pak Inertial Motion Unit (IMU). Data 
from the DVL and IMU are fused using an EKF to provide a high quality dead reckoning 
solution while submerged. While surfaced, ARIES utilizes Differential Global 
Positioning System (DGPS) to correct any navigational errors accumulated during the 
submerged phases of a mission. 

The EKF in the ARIES’ navigational suite may be tuned for optimal performance 
given a set of data. By fusing data input from the IMU / DVL / DGPS suite, biases, such 
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as yaw rate bias and compass bias, can be identified and compensated for during 
underwater travel. Although this compensation can not completely correct for 
environmental biases, a relatively short surface time, for example, 10 seconds allows the 
filter to re-estimate biases, correct position estimates and continue with improved 
accuracy (Marco, Healey, 2000). 

For obstacle avoidance and target acquisition, the ARIES uses scanning or 
profiling sonar. The sonar heads can scan continuously through 360° of rotation or 
sweep through a defined angular sector (Marco, Healey, 2000). 

B. COMMUNICATIONS 

ARIES is equipped with two types of modems. Radio modems are used for high 
bandwidth command/control and system monitoring while the vehicle is surfaced. An 
acoustic modem is used for low bandwidth communications while the vehicle is 
submerged. Additionally, the acoustic modem enables ARIES to exchange pertinent data 
with other AUVs that have compatible components (Marco, Healey, 2000). 

C. SERVER VEHICLE CONCEPT 

This thesis models the “master” vehicle the server of a two vehicle network. 
Proposals for using the NPS ARIES as a network server vehicle for multi-vehicle 
cooperative operations have surfaced due to the foreseen benefits. Use of the server 
vehicle as a data relay increases the range of communications of the underwater 
components of the network. Figure 2.2 describes the concept where in position 1, the 
ARIES communicates through its acoustic modem with multiple worker vehicles that are 
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engaged in a search pattern. Position 2 shows the ARIES on the surface using a radio 
modem to report mission status of the worker vehicles (possibly vehicle positions, image 
snippets of targets, and hydrographic data) to the command ship. While surfaced the 
server vehicle can receive tactical decision re-tasking commands. Once the new orders 
are received, the vehicle will submerge and transmit, using its acoustic modem, new tasks 
to each worker vehicle. Using a server vehicle eliminates the complexity of deploying 
fixed buoys. Also, a vehicle of this type can achieve close proximity or rendezvous with 
the worker vehicles allowing for higher acoustic bandwidth data transfer (Marco, Healey, 
2000 ). 
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Figure 2.1 - Hardware Components of the NFS ARIES (Marco, Healey, 2000) 
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Figure 2.2 - Sever vehicle concept. 1. Low bandwidth submerged data transfer 
between underwater vehicles. 2. High-speed data relay to command ship (Marco, 

Healey, 2000). 
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m. RANGE ONLY MEASUREMENT (ROM) THEORY 



A. INTRODUCTION 

Only in the past few years has serious research in Range Only Measurement 
(ROM) jarget tracking been conducted. Seen as a means to reduce the cost of active 
target tracking, methods of ROM target tracking are increasingly being investigated. 
ROM target tracking previously lacked interest of researchers due to “the lack of firm 
analytical basis for system observability in addition to the lack of proper filter structures 
in the field of target tracking with ROM from one observer.” (Song, 1999) 

Previous study also found that, “global system observability! may not exist for the 

ROM target tracking; however, target state variables such as position, velocity, and 

P\\ '-r . , 

acceleration can be estimated with a proper initial state estimate due to local system 

observability.” (Song, 1999) If available, an initial state estimate can be obtained from 
previous target data, an update from an external source, i.e. a surface ship, or through the 
use of the “master” vehicle’s active sonar. 

This study furthers the above by showing that accurate position estimates can be 
obtained without an accurate initial position, and that global observability for ROM target 
tracking is possible under certain conditions. 

B. PROBLEM FORMULATION 

To properly investigate the questions concerning ROM’s accuracy and 
observability an appropriate model had to be constructed. Since this tracking technique 
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may be used on an AUV, the NPS ARIES AUV provided the basis for model vehicles’ 
capabilities and constraints. 

Modeling of the ROM system was done in computer based simulations using 
MATLAB due to its strong processing and presentation capabilities. Two simulations 
were developed; each models the same problem with a few changed parameters. 
Development of the simulation models was split into two sections: 

1 . The dynamic model 

2. The Extended Kalman Filter (EKE) 

The development and theory behind constructing the above sections will be explained in 
detail. 



1. The Dynamic Model 

The simulations model a system consisting of two AUVs, a “master” and a 
“drone” vehicle operating in a zero current environment. The depths of the two vehicles 
were disregarded as this is assumed to be only a two dimensional problem. The “master” 
AUV navigation suite matches that of the AIRES described in the previous chapter, and 
is assumed to have accurate position, velocity, acceleration, and course data at all times. 
On the contrary, the “drone” vehicle possesses a rudimentary navigation suite consisting 
of an off the shelf compass and a bottom-mounted Doppler Sonar Velocity Log. 
Combining its compass heading, speed over ground, and an initial estimated position, the 
drone uses dead reckoning to estimate and update its position. 

For both simulations, almost ideal surroundings were used to model the 
environment. As stated above, both simulations were modeled without current or other 
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external forces, i.e., wind, waves, etc. To introduce an error between the actual and the 
dead reckoning courses, the compass was assumed to have a constant bias of five degrees 
east. The bias may seem artificially large, but testing with the AIRES has proven that 
this value represents actual bias errors experienced using comparable compasses. Other 
environmental factors that would affect the vehicles’ motion were made negligible in 
order to focus on the details more critical to this thesis. 

To provide the best conditions for ROM observability, careful selection of vehicle 
paths was necessary especially that of the “master” vehicle. In previous study. Dr. Taek 
Song found that; 

...if the tracker-target relative motion results in a constant bearing 
trajectory, or if the tracker is moving with a constant velocity or a constant 
acceleration, the system is not observable. This implies that the tracker 
motion should contain nonzero jerk to track a target with constant 
acceleration. (Song, 1998) 

Based on this observation, it was concluded that a circular path of some sort at maximum 
cruising speed would provide the best results by satisfying Dr. Song’s criteria in tracking 
linear paths. It was decided to make the “mastef’/tracker vehicle’s path a circle. In 
doing this, the “master” AUV’s bearing, opening, and closing rates relative to the "drone" 
AUV could be altered by changing its path radius. In all cases, the “master” AUV’s 
speed of one and one-half meters per second remained constant. 

A more straight forward task, the “drone”/target vehicle path selection represents 
an AUV conducting a portion of its minesweeping search pattern at a constant speed of 
two-tenths meters per second. For the Case One simulation, the “drone’s” dead 
reckoning and actual positions start in the center of the “master” vehicles circular path. 
The desired and dead reckoning course is due north, course 000, but due to the compass 
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error the actual path taken by the “drone” is five degrees east or course 005. Figure 3.1 
illustrates the three paths described above. The Case Two simulation assumes the actual 
initial position of the “drone” is unknown, but located near the circular path of the 
“master” vehicle. This was done using MATLAB's, “randn” function which produces 
random entries chosen from a normal distribution with a zero mean and a variance of one. 
As in the Case One simulation, the actual drone model moves from its initial position on 
a course of 005 due to its compass bias. The “drone’s” dead reckoning position starts in 
the center of the “master” vehicle’s path and follows the desired due north course. Figure 
3.2 shows the AUV paths in a typical Case Two simulation. 

For each simulation, a five element state vector in the Cartesian coordinates 
consisting of two “master” AUV position states, [X, Y\, two “drone” AUV’s position 
states, [xi, y;], and the “drone” AUV’s heading state, [i}/], represented the system. The 
continuous dynamic system is expressed as 

x = Ax+Bu 

SI- 

where -.x = (A:,)^x,,>-,,\i/)^, 
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■ 0 ■ 
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and 




in which Rs equals the radius in meters of the “master” AUV’s continuous circular path 
at speed Um. It was assumed that the tracking system dynamics are represented perfectly 
so that the process noise is not included in this analysis, (Song, 1999). 
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The next step in the problem formulation was converting the continuous dynamic 
system into a discrete dynamic system so it would be compatible with the EKF. 
MATLAB’s continuous to discrete function, “c2d”, eased the process considerably. The 
basics of the “c2d” algorithms are taken from the Control Toolbox and described here for 
background on the process. 

To convert a state space linear time invariant (LTI) system expressed as 

x = Ax+Bu 

into a discrete time state space system represented as 

Xk+i = 0Xk + ruk 

Uk = l 



MATLAB calculates the matrix exponential phi, ^(dt), as 

<D = e'^‘" 

dt = the sample time increment 

and gamma, as a value that maps inputs to system response such that 

r=(0-I)A-'B. 

With the values of phi and gamma, recursive loops in the simulation program calculate 
the discrete vehicle states for each increment of the specified simulation time length, t. 

2. Kalman Filtering 

The Kalman filter is a set of mathematical equations that provides an efficient 
recursive solution of the least-squares method. The filter is very powerful in several 
aspects: it supports estimations of past, present, and even future states, and it can do so 
even when the precise nature of the modeled system is unknown (Bishop and Welch, 
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1995). The algorithm computes the best estimates of system variables arising from 
sensor based data and the system model. Data from measurements along with the 
measurement model are used in a system model to provide the least squares fit estimate 
of system state, based on those measurements (Healey, An, Marco 1998). The Kalman 
filter also assumes there is a zero mean error associated with the measurement data and 
estimation process. Based on a priori information, the modeler has the ability to adjust 
these error values to increase simulation accuracy. 

Due to the nonlinear nature of equations used in the simulations, an EKF was 
used to fuse the available data and provide position estimates. The system is a 
continuous time model of vehicle motion represented by 

i(/) = m/))+^(0; 

j(f) = M4t))+v(/); 

where x(t)e is the model state, / and h are continuous functions differentiable by 
x(t), and q and v are zero mean white noise excitations for the system and measurement 
models respectively (Healey, An, Marco, 1998). For the “master” AUV, the states are 
globally referenced longitude and latitude in meters, X and Y, the “drone” AUV states are 
also globally referenced, actual, longitude and latitude in meters, xi and yi, and the 
heading angle referenced to North, xj/ (Stinespring, 2000). The filter state vector, x, is 
made up of the above states, and the system is of order five. 

f > .^1 j 9 

The state model is related through the following set of functions representing 
dynamic relationships between states with assumptions embodying maneuvering models: 
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X =(oy 

y = -(ox 

x^ = 0 ; 
ji =0; 
v = 0; 



To calculate the above state functions and solve the equations, the EKF takes the 
outputs from some of the “master” AUV’s sensors measurements. The particular sensors 
that provide measurement data at that time are related to the filter state with the C matrix 
(Healey, An, Marco, 1998). This matrix is also used to convert estimated state vectors 
into the output matrix y. This matrix is in the form: 

y^=[Range, X, Y, x,, yj, 

The equations, which are related in the C matrix and determine the output based on what 
the system is receiving from the sensors are as follows (Stinespring, 2000): 



The output yi is range between the “master” and “drone” vehicles at the time 
increment k, y 2 .s are the “master” vehicle’s position components as recorded from DGPS, 
y 4 j are the “drone” position components as reported by their onboard position estimators, 
and y6 is the “drone” AUV’s heading as reported from its compass. The output is then 
used to refine the drone position based on the sensor outputs. It was assumed that ranges 
calculated by the acoustic modem signal travel times would be accurate. Ranges used by 
the EKF in the simulations are exact and utilize data that would not be available to the 
filter in real world situations. 




3-4 =^i; 
>5 =yi; 

^6 =v; 
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The C matrix composition is the only difference in the EKF between the Case 
One and Case Two simulations. In the Case One simulation, the C matrix’s construction 
relates data from all sensors channels to EKF. This simulation provides more data than 
the EKF needs to provide an accurate position estimate, but it served as excellent 
program to tune the EKF. In the Case Two simulation, the C matrix only allows channels 
one through three, and six to be used by the EKF. The remaining channels were made 
unusable to the EKF by zeroing the applicable coefficients in the C matrix. 

As stated before, the simulations represent near ideal systems. The system and 
measurement noise matrices of Q and R, respectively, were kept relatively small and can 
be considered to have little to no effect on the results for both simulations, except to 
determine the response time of the EKF. 

3. Observability Determination 

EKF solution observability is probably the most important parameter in whether 

the data from the EKF is valid. Observability of the filter is essential in order to 

guarantee stable unbiased estimation errors of the state. A nonlinear filter such as the one 

used in the simulations, can be verified for observability locally through linearization of 

the / and h functions (Healey, An, Marco, 1998). For a linearized model, 

x(t) = Ax(t) + q; 
y{t) = CxU) + v, 

where A=— and C = — ; 

dx dx 

the system is locally observable if the following observability matrix, O, has full rank of 
five. 
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0=[C\C’A\CA’^ ,C’A’\C'A'\CA'^] 



To calculate the total system observability, the integral of the observability grammian 
over the entire simulation time period, t, must be of full rank. 

rank(\li^'^{xfi)C'^{x)C{zmx,Q))dt)) = n\ 
where n=length(x)\ 

Observability matrices of full rank in each simulation provide an excellent basis for 
establishing ROM’s observability. 
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IV. RESULTS OF SIMULATIONS 



A. INTRODUCTION 

During development and data collection, several server vehicle speeds and 
circular path radii were used. As a means to limit the amount of data collected, vehicle 
speeds were kept constant and turning capabilities were based on those of the AIRES 
AUV. 

Two simulation scenarios were modeled to test the ROM EKF. In the Case One 
scenario, the starting position of drone vehicle was constant and accurately passed to the 
EKF. Additionally, by modifying the C matrix, described in the previous chapter, the 
EKF used all the channels of measured sensor data to more accurately calculate the 
“drone” AUV’s position. In the Case Two scenario, it was intended to model a system in 
which the starting position of the drone AUV was unknown, and it’s position data was 
not available to the EKF. 

To further characterize the accuracy of the EKF, the simulations were run with 
“master” AUV path radii of 17, 30, 50, and 100 hundred meters. The speed of the 
“master” vehicle was kept constant at one and one-half meters per second, which is the 
AIRES’ maximum speed. 

B. CASE ONE MODEL 

In this scenario, the “drone” AUV’s actual and dead reckoning position started at 
the center “master” AUV’s circular path. As the simulation progresses, the desired and 
actual paths of the “drone” AUV diverge due to compass bias as shown in Figure 3.1. 
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The C matrix composition allows the actual position and heading states of the “drone” 
vehicle to pass to the EKF. 

1. Observability 

In all Case One simulations, the system exhibited complete observability. The 
system’s local observability matrix and observability grammian were always of full rank. 
This was to be expected since all of the filter states were updated by sensor data at every 
time increment. Figures 4.1 and 4.2 depict the local and total system observability, 
respectfully. These figures are applicable to all “master” AUV path radii conducted in 
this scenario. 



2. ROM Accuracy 

Configured to relate data from all measurement channels, the EKF made accurate 
position approximations, but a noticeable oscillatory error in the east-west/cross track 
accuracy plagued runs with larger “master” AUV paths. Further testing determined that 
the error was related to the speed in which the “master” AUV completed its circular path 
or in other words its cyclic speed. Since the “master” AUV’s speed was kept fixed, 
modifying its path radius was the only means to effect cyclic speed. Decreasing the 
radius of “master” AUV’s path increased its cyclic rate. Accuracy of the EKF improved 
dramatically as the cyclic rates of the “master” AUV were increased. The difference in 
position between the actual and estimated “drone” AUV position drops considerably as 
illustrated in the following figures. Figure 4.3 compares the actual east-west position to 
the EKF estimated position for a “master” AUV path radius of 100 meters. Figure 4.4 



24 



compares the actual north-south position to the EKF estimated position for a “master” 
AUV path radius of 100 meters. Figures 4.5 through 4.10 show the same information as 
figures 4.4 and 4.5, for the three other “master” AUV paths. 

In comparing ROM position estimation to dead reckoning, ROM demonstrated 
itself as a superior. Unlike dead reckoning, which may produce unbounded position 
errors, ROM position estimation solutions converge if given sufficient time. For short 
periods of time, dead reckoning provided positions that rival the accuracy of ROM 
position estimation. However, for periods longer than approximately 1000 seconds, 
ROM position estimates mirror the “drone” AUV’s actual position much more closely 
than dead reckoning. 

Figure 4.11 shows the paths of the “master” AUV, the “drone” AUV’s desired 
and actual paths, and the ROM position estimate path. This figure clearly depicts ROM’s 
accuracy over dead reckoning. To further show ROM’s accuracy over dead reckoning. 
Figure 4.12 illustrates the normalized differences between the dead reckoning and actual 
positions and the ROM estimated and actual positions. Figure 4.13 shows the error of 
ROM position estimation states is bounded, and over time decreases as it converges. 
Figures 4.14 through 4.22 illustrate the information displayed in Figures 4.11 through 
4.13 for the other “master” AUV path radii. These figures clearly show that as the 
“master” AUV’s cyclic rate increases, the accuracy of position estimation also increases. 

C. CASE TWO MODEL 

It was intended for this portion of the simulation to model a scenario in which the 
“master” AUV did not have an accurate “drone” AUV initial position. To further 
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simulate real world conditions, the only data made available to the EKF was the “drone” 
AUV range and heading data and the “master” AUV’s position. 

1. Observability 

In all no dead-reckoning simulations, the rank of the local observability matrix at 
any point in time was four. Unlike the previous scenario, “drone” vehicle position was 
not one of the sensor measurements used to update the EKF. Therefore its states became 
locally unobservable. Using the observability grammian to calculate observability over 
the entire simulation time span, the number of observable states increased to five. 
Initially, the drone’s ability to send its heading seems trivial. However, without this data 
it would not be possible for the observability grammian to reach full rank. Figures 4.23 
and 4.24 show the system’s local observability and total observability, respectively. 
These figures are applicable to all “master” AUV path radii conducted in this scenario. 

2. ROM Accuracy 

Unlike the simulations with known initial “drone” positions, the EKF did not 
produce accurate estimates in the early stages of the simulations. It generally took one 
“master” AUV cycle before gaining acceptable position data. Initially, the EKF appears 
slow to adjust to the “drone’s” actual position due to their proximity and minimal change 
in range, but once the range and range rate increase the EKF quickly “zeroes in” on the 
drone AUV’s actual position. As demonstrated in the previous model, when the cyclic 
rate of the “master” AUV increased, the accuracy of the EKF also increased. 
Additionally, increasing the “master” AUV’s cyclic rate allowed a faster acquisition of 
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the “drone” by the EKF. They become smaller and recover more quickly as cyclic rate 
increases. This is illustrated by characterizing the initial spikes in the EKF’s position 
estimation prior to obtaining an accurate solution in the following figures. Figure 4.25 
compares the actual east-west position to the EKF estimated position for a “master” AUV 
path radius of 100 meters. Figure 4.26 compares the actual north-south position to the 
EKF estimated position for a “master” AUV path radius of 100 meters. Figures 4.27 
through 4.32 show the same information as figures 4.25 and 4.26, for the three other 
“master” AUV paths. The above figures show that as the “master” vehicle’s path radius 
decreases, the initial position error displayed by the spikes lessens and accurate position 
estimates occur quicker. 

Again, ROM target tracking proved to be a more accurate means of position 
estimation than dead reckoning for longer periods of time. For shorter periods of time, 
dead reckoning provided more accurate position estimations. Unlike the previous model, 
there was not a general time under which dead reckoning was more accurate. Since the 
EKF took approximately one cycle to build accurate position solutions, the time in which 
the dead reckoning solution was more accurate varied. 

Once again, decreasing the “master” AUV path radius increased the accuracy of 
the EKF position estimations. This effect is shown clearly in the following figures. Due 
to the random initial position generated by the program, figures representing any single 
“master” AUV path radius may not be taken from the same simulation. Figure 4.33 
shows the paths of the “master” AUV, and the “drone” AUV’s dead reckoning, 
estimated, and actual paths. Figure 4.34 shows the normalized difference between dead 
reckoning and actual position and the normalized difference between ROM and actual 
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positions. Figure 4.35 shows the error between the actual and estimated position states of 
the “drone” AUV. Figures 4.36 through 4.44 illustrate the information displayed in 
Figures 4.33 through 4.35 for the other “master” AUV path radii. 
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Figure 4.2 - Total observability for all “master” AUV path radii 
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Figure 4.3 - East- west EKF position estimates vs. actual position for 100 meter 

“master” AUV path radius 
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Figure 4.4 - North-south EKF position estimates vs. actual position for 100 meter 

“master” AUV path radius 
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Figure 4.5 - East- west EKF position estimates vs. actual position for 50 meter 

“master” AUV path radius 
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Figure 4.6 - North-south EKF position estimates vs. actual position for 50 meter 

“master” AUV path radius 
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Figure 4.7 - East-west EKF position estimates vs. actual position for 30 meter 

“master” AUV path radius 



35 



( 

I 

4 



400 



ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION 




Figure 4.8 - North-south EKF position estimates vs. actual position for 30 meter 

“master” AUV path radius 
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Figure 4.9 - East-west EKF position estimates vs. actual position for 17 meter 

“master” AUV path radius 
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Figure 4.10 - North-south EKF position estimates vs. actual position for 17 meter 

“master” AUV path radius 
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Figure 4.11 - AUV paths over 3000 seconds 



39 



I 



I 



i 

I 

I 

I 

I 



NORMALIZED DIFFERENCES IN POSITION AT TIME (T) 




Figure 4.12 - Normalized differences between indicated and actual position for 100 

meter “master” AUV path radius 
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Figure 4.13 - Difference between actual and estimated position states for 100 meter 

“master” AUV path radius 
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Figure 4.14 - AUV paths over 2000 seconds 
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Figure 4.15 - Normalized differences between indicated and actual position for 50 

meter “master” AUV path radius 
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Figure 4,16 - Difference between actual and estimated position states for 50 meter 

“master” AUV path radius 
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Figure 4.17 - AUV paths over 2000 seconds 
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Figure 4.18 - Normalized differences between indicated and actual position for 30 

meter “master” AUV path radius 
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Figure 4.19 - Difference between actual and estimated position states for 30 meter 

“master” AUV path radius 



47 



AUV PATHS 




Figure 4.20 - AUV paths over 2000 seconds 
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Figure 4.21 - Normalized differences between indicated and actual position for 17 

meter “master” AUV path radius 
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Figure 4.22 - Difference between actual and estimated position states for 17 meter 

“master” AUV path radius 
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Figure 4.23 - Local Observability for all “master” AUV path radii with no initial 

drone condition data 
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Figure 4.24 - Total observability for all ‘‘master” AUV path radii with no initial 

drone condition data 
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Figure 4.25 - East-west EKF position estimates vs. actual position for 100 meter 
“master” AUV path radius with no initial position data 



53 




I 



700 



ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION 




Figure 4.26 - North-south EKF position estimates vs. actual position for 100 meter 
“master” AUV path radius with no initial position data 
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Figure 4.27 - East-west EKF position estimates vs. actual position for 50 meter 
“master” AUV path radius with no initial position data 
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Figure 4.28 - North-south EKF position estimates vs. actual position for 50 meter 
“master” AUV path radius with no initial position data 
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Figure 4.29 - East-west EKF position estimates vs. actual position for 30 meter 
“master” AUV path radius with no initial position data 
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Figure 4.30 - North-south EKF position estimates vs. actual position for 30 meter 
“master” AUV path radius with no initial position data 
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Figure 4.31 - East-west EKF position estimates vs. actual position for 17 meter 
“master” AUV path radius with no initial position data 
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Figure 4,32 - North-south EKF position estimates vs. actual position for 17 meter 
“master” AUV path radius with no initial position data. 
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Figure 4.34 - Normalized differences between indicated and actual position for 100 

meter “master” AUV path radius 
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Figure 4.35 - Difference between actual and estimated position states for 100 meter 

“master” AUV path radius 



63 



AUV PATHS 




64 



I 



I 







200 



NORMALIZED DIFFERENCES IN POSITION AT TIME (T) 




Figure 4.37 - Normalized differences between indicated and actual position for 50 

meter “master” AUV path radius 
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Figure 4.38 - Difference between actual and estimated position states for 50 meter 

“master” AUV path radius 
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Figure 4.40 - Normalized differences between indicated and actual position for 30 

meter “master” AUV path radius 
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Figure 4.41 - Difference between actual and estimated position states for 30 meter 

“master” AUV path radius 
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Figure 4.43 - Normalized differences between indicated and actual position for 17 

meter “master” AUV path radius 
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Figure 4.44 - Difference between actual and estimated position states for 17 meter 

“master” AUV path radius 
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V. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

This work, through computer modeling and simulation, has shown that ROM 
position estimation will provide more accurate position estimates than dead reckoning. 
Results from this work also demonstrated that ROM position estimation produces 
observable results. From previous work, it was known that the server path must contain a 
non-zero acceleration motion to enable the EKF to find accurate, observable solutions 
(Song, 1999). However, the results also manifest that the accuracy of the solution is also 
dependent on the tracker vehicle’s path. For the circular paths traveled by the master 
vehicle, larger non-zero accelerations translated to smaller circular paths higher bearing 
and range rates relative to the drone. As these rates increased, accuracy also increased. 

B. RECOMMENDATIONS 

The work and research completed in this thesis utilized simulations that were 
nearly ideal. Based on positive results gathered, further study should be made in both 
development of the EKF and the dynamic model. 

Further optimization of the EKF to reduce the oscillatory errors would greatly 
improve estimated position accuracy. Also testing the EKF’s accuracy when current and 
realistic noise values are modeled could prove very valuable in determining if this system 
could be used on an AUV. 

Future research to enhance the dynamic model should include simulations with 
two or more drones acting independently or simulations with one drone making course 
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changes. Lastly, a vital issue requiring more research is finding alternate master vehicle 
paths that create observable solutions and that patrol operation areas effectively. 
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APPENDIX A. MATLAB CODE ASSOCIATED WITH KNOWN 
INITIAL POSITION SIMULATION 



% Jason Alleyne 
%initpos.m 

%2 AUV Range only Measurement Simulation 
%Known Initial Drone Position 



clear 

clc 



%System dynamic model of worker measurement process for the range measurement 



u=L5; 

Rs=100; 

w=u/Rs; 

psi=0; 

b=(pi/180)*5; 

q=[0.2;0;0]; 

Aw=[0,0,0;0,0,0;0,0,0]; 

xl=zeros(3,2000);xla=zeros(3,2000); 

xl(:,l)=[0;0;0];xla(:,l)=[0;0;0]; 



%server vehicle speed, u=1.5 m/s 

%Server vehicle path circular radius, and angular velocity 
%the worker vehicle ordered course 
%the worker vehicle compass bias, 5 degrees 
%the worker speed is .2 m/s 



%initializing the actual (xla) and dr (xl) position vectors 
^initial posits/states at the origin for the simulations. 



speed=zeros( 1 ,2000);compass=zeros( 1 ,2000); 



%initializing the speed and compass vectors 



dt=l; 

[p2,g2]=c2d(Aw,q,l); 



%time step of one second 

%continuous to discrete function for dr course 



for i=l;(length(xl)-l); 
xl(:,i+l)=p2*xl(:,i)+g2; 
speed(i)=g2(l);' 
compass(i)=x 1 (3,i)+b; 
f 1 (i)=speed(i)*cos(compass(i)); 
f2(i)=speed(i)*sin(compass(i)); 
q2=[fl(U);f2(l,l);0]; 
[p3,g3]=c2d(Aw,q2,l); 
xla(:,i+l)=p3*xla(:,i)+g3; 

V(i)=xl( 1 ,i)-x la(l ,i);X(i)=xl(2,i)-xla(2,i); 
DIFF(i)=sqrt(V(i)^2+X(i)^2); 
end 



%Initializes speed array for D.R. 

%Initializes compass array for D.R. 

%The actual speed/heading due to compass bias 
%The actual speed/heading due to compass bias 
%Used as propagation q matrix 
%C2D to convert continuous actual to discrete actual 
%Actual track of worker 



%circular motion of the server radius, Rs. 

% xO is the position vector [x,y] of the server 

% Set up circular path for server. Example is radius of 100 meters at w=0.015 rad/sec. 
%System dynamic model of server measurement process for the range measurement 



As=[0, w;- w,0] ;q=[ 1 ; 1 ] ; 
x0=zeros(2,2000);x0(:,l)=[Rs;0]; 

[p,g]=c2d(As,q,l); 
for i=l:(length(x0)-l); 
x0(:,i+l)=p*x0(:,i); 

R(i)=sqrt((x0(l,i)-xl(l,i))^2+(x0(2,i)-xl(2,i))^2); %Range between DR worker position and server 

%vehicle; 

Ra(i)=sqrt((x0(l,i)-xla(l,i))^2+(x0(2,i)-xla(2,i))^2); %actual Range between worker and server vehicles; 
end 
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figure(2),clf,plot(xO(2,:),xO(l,:X’b-\xl(2,:),xl(l,:),’g.’),grid 
xlabel(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 
ylabel(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 
title(\fontname{ courier new}\fontsize{ 12}\bf AUV PATHS’); 
legend( ’Master AUV Path’, Desired Drone AUV Path’), axis equal 



figure(3),clf,plot(R,’g*’),grid 

hold;plot(Ra,’b-’); 

xlabel(’Vfontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabel(\fontname{ courier new}\fontsize{ 12}\bf RANGE (METERS)’); 
title(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL VS DEAD RECKONING RANGE’); 
legend(Dead Reckoning Range’, ’Actual Range ’,0) 

figure(4),clf,plot(xl(2,:),xl(l,:),’g.-’,xla(2,:),xla(l,:),’r*’,x0(2,:),x0(l,:),’b-’)»grid,axis equal, 

xlabel(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 

ylabel(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 

title(\fontname{ courier new}\fontsize{ 12}\bf AUV PATHS’); 

legend( Desired Drone AUV Path’, ’Actual Drone AUV Path\Master AUV Path’,0) 

%build an Kalman filter for master vehicle 

% Position Estimator based on data stream, R, 

% 

endSample=length(Ra);startSample= 1 ; 

y=[Ra;xO( 1,1: length(Ra));xO(2, 1 :length(Ra));x 1 a( 1 , 1 :length(Ra)) ;x 1 a(2, 1 :length(Ra));x 1 a(3, 1 :length(Ra))] ; 

x=zeros(5,endSample);err=zeros(6,endSample); 

s=startS ample; 

x(:,s)=[x0( 1 , 1 ),x0(2, 1 ),0,0,0] ’; 



%M ANEUVERING AND CURRENT TIME CONSTANTS 

taul=0; 

tau2=0; 

A=[As,zeros(2,3);zeros(3,2),Aw]; 

C=zeros(6,5); %Initializes the C matrix 

C(2,1)=1;C(3,2)=1;C(4,3)=1;C(5,4)=1;C(6,5)=1; %Matches measurement states (y) to output states (x) 

Ra( 1 )=sqrt((xO( 1 , 1 )-x 1 a( 1 , 1 ))'^2+(x0(2, 1 )-x 1 a(2, 1 ))^2) ; , 

% C matrix local is dg/dx 

C( 1 ,3)=-(x0( 1 , 1 )-x 1 a( 1 , 1 ))/Ra( 1 ); 

C( 1 ,4)=-(x0(2, 1 )-x 1 a(2, 1 ))/Ra( 1 ); 

C(l,l)=(xO(l,l)-xla(l,l))/Ra(l); 

C( 1 ,2)=(x0(2, 1 )-x 1 a(2, 1 ))/Ra( 1 ); 

%Initial B matrix 
q 1=0.01; 
q2=0.01; 
q3=0.1; 
q4=.l; 
q5=0.0001; 



%variance on X, m^^2 
%variance on Y, m^2 
% variance on xla 
% variance on yla, rad/s)^2 
%slow bias convergence 
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B=[q 1 ;q2;q3;q4;q5]/l 00; 

Q=diag(B); %sysiem noise 

nul=0.01 ;nu2=.001 ;nu3=.001 ;nu4=2000;nu5=2000;nu6=.02; 



gnu=[nul;nu2;nu3;nu4;nu5;nu6]; 

R=diag(gnu)*l; %measurement noise 

Gram=zeros(5,5); %initialization of observability grammian matrix 

% measured data at old time, new__before means model predicted value 

p_old_after=eye(5); 
delx_old_after=zeros(5, 1 ); 

g=ones(6,l);psave=zeros(5,length(R)); 

dt= 1 ;phi=expm(A*dt); t=0: 1 rendSample- 1 ; 



for i=startSample:(endSample- 1 ); 



%compute linearized PHI matrix using updated A 
%reset initial state 

x_old_after=x(:,i); 

% nonlinear state propagation 

[x__new_before]=phi*x_old_after; 

%error covariance propagation 

p_new_before=phi*p_old__after*phi’ + Q; %new gain calculation using linearized new 

%C matrix and current state error 
%covariances. 



%formulate the innovation using nonlinear output propagation 
%as compared to new sampled data from measurements 

y hat=o utput3 (x_ne w_before) ; 
err(:,i+l)=(y(:,i+l) - yhat); 

G=p_new__before*C’*inv(C*p_new_before*C' + R); % Kalman Gain 

p_old_after=[eye(5) - G*C]*p_new_before; 
psa ve( : ,i+ 1 )=d iag(p_o ld_after ) ; 

cpc=inv(C*p_old_after*C’+R); 

rel(i+l)=err(:,i+l)’*cpc*err(:,i+l); 

x_new_after=x_new__before + G*err(:,i+1); 
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%carry new state into next update 
x(:,i+l )=x_new_after; 

C=zeros(6,5); 

C(2,1)=1;C(3,2)=1;C(4,3)=1;C(5,4)=1;C(6,5)=1; %Current settings make all target states observable to 

%filter. 



% Cmatrix local is dg/dx 

C(l,3)=-(x(ld)-x(3,i))/y(l,i); 

C(l,4)=-(x(2,i>x(4d))/y(ia); 

C(U)=-C(1,3); 

C(l,2)=-C(l,4); 

0(i)=rank([C;phi’*Cl); 

U(i)=x(4,i)-y(5d);T(i)=:x(3d)-y(4,i); 

diff(i)=sqrt(U(i)^2+T(i)'^2); 

Gram=Gram+phi’*C’*inv(R)*C*phi; 

Gramrank(i)=rank(Gram); 

end; 

figure(5),clf,plot(0,’b*’), grid, zoom %Local Observability indicates # of observable states at a point in 

%time. 

xIabeI(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabeI(^fontname{ courier new}\fontsize{ 12}\bf OBSERVABLE STATES’); 
title(’Vfontname{ courier new } \fontsize{ 12}\bf LOCAL SYSTEM OBSERVABILITY’) 

figure(6),clf,plot(Gramrank,’b*’), grid, zoom %Observability Grammian Rank indicates # of observable 

%states over period of the simulation 
xlabel(’Vfontname{ courier new }\fontsize{ 12}\bf TIME (SEC)’); 
ylabel(’Vfontname{ courier new}\fontsize{ 12}\bf OBSERVABLE STATES’); 
title(’Vfontname{ courier new}\fontsize{ 12}\bf TOTAL SYSTEM OBSERVABILITY’) 

figure(7),clf,pIot(t,rel,’b*’),grid,zoom 

figure(8),clf,plot(t,x(4,:),b*’,t,xla(2,l:1999),’m.’),grid %East-West position comparison 

xlabel(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabel(’Vfontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 

title(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL vs. ESTIMATED EAST-WEST POSITION’); 
legend( Estimated position’, ’Actual position’,0) 

figure(9),clf,plot(t,x(3,:),’b*’,t,xia(l,l: 1999), ’m.’), grid %North-south position comparison 

xlabeI(\fontname{ courier new}\fontsize{ 12)\bf TIME (SEC)’); 
ylabel(’Vfontname{courier new}\fontsize{ 1 2 }\bf METERS’); 

title(’Vfontname{ courier new}\fontsize{ 12)\bf ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION’); 
legend( Estimated position’, ’Actual position’,0) 

figure( 1 0),clf,plot(x(2,:),x( 1 ,:)),grid,hold, 

plot(x(4,:),x(3,:),’r. ’),plot(x 1 (2,:),x 1 ( 1 ,:),’m.’),plot(x 1 a(2, 1 : 1 999),x 1 a( 1 , 1 : 1 999), ’c. ’);zoom,hold off, axis 
equal 

xlabel(\fontname{ courier new}\fontsize{ 1 2 )\bf METERS’); 
ylabel(’Vfontname{ courier new}\fontsize{ 1 2 )\bf METERS’); 
title(\fontname{ courier new}\fontsize{ 12)\bf AUV PATHS’); 

legend( ’Master AUV Path’, Estimated Drone Path’, Desired Drone AUV Path’, ’Actual Drone AUV Path’,0) 
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figure(l l),clf,plot(err’),grid, 

xIabel(\fontname{ courier new}\fonisize{ 12}\bf TIME (SEC)’); 
ylabel(\fontname{ courier new}\fonlsize{ 1 2 }\bf METERS’); 

title(\fontname{ courier new}\fonisize{ 12 }\bf DIFFERENCE BETWEEN ACTUAL AND ESTIMATED 
VALUES’); 

legend ( Han ge\ 'Server North-South’, ’Server East-West', Worker North-South’, Worker East-West’, Worker 
Heading ’,0) 



figure(12),clf,plot(diff,’r-’),grid,hold,plot(DIFF,’b.’),hold off 
xlabel(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabeI(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 

title(\fontname{ courier new}\fontsize{ 1 2 }\bf NORMALIZED DIFFERENCES IN POSITION AT TIME 

(T)0; 

legend(’ROM Position Estimate’, Dead Reckoning’, 0) 
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APPENDIX B. MATLAB CODE ASSOCIATED WITH UNKNOWN 
INITIAL POSITION SIMULATION 



%Jason Alleyne 
%no-initpos.m 

%2 AUV Range only Measurement Simulation 
%Unknown Initial Drone Position 



clear 

clc 



%System dynamic model of worker measurement process for the range measurement 



u=1.5; 

Rs=100;w=u/Rs; 
psi=0; 

b=(pi/180)*5; 
q=[0.2;0;0]; 

Aw=[0,0,0;0,0,0;0,0,0]; 

X 1 =zeros(3 ,2000) ;x la=zeros( 3 ,2000) ; 



%server vehicle speed, u=1.5 m/s 

%Server vehicle path circular radius, and angular velocity 
%the worker vehicle ordered course 
%the worker vehicle compass bias, 5 degrees 
%the worker speed is .2 m/s 



xl(:,l)=[0;0;0];I=randn([2,l])*Rs;xla(:,l)=[l;0]; 
initial value. 

speed=zeros( 1 ,2000);compass=zeros( 1 ,2000); 
dt=l; 

[p2,g2]=c2d(Aw,q,D; 



%initializing the actual (xla) and dr (xl) position vectors 



%The actual position is unknown, and is a random 



%initializing the speed and compass vectors 

%time step of one second 

%continuous to discrete function for dr course 



for i=l:(length(xl)-l); 
xl(:,i+l)=p2*xl(:,i)+g2; 
speed(i)=g2(l); 
compass(i)=xl(3,i)+b; 
f 1 (i)=speed(i)*cos(compass(i)); 
f2(i)=speed(i)*sin(compass(i)); 
q2=[fl(l,l);f2(l,l);0]; 
[p3,g3]=c2d(Aw,q2,l); 
xla(:,i+l)=p3*xla(:,i)+g3; 
V(i)=xl(l,i)-xla(l,i);X(i)=xl(2,i)-xla(2,i); 
DIFF(i)=sqrt(V(i)^2+X(i)'^2); 
end 



%Initializes speed array for D.R. 

%Initializes compass array for D.R. 

%is the actual speed/heading due to compass bias 
%is the actual speed/heading due to compass bias 
%used as propagation q matrix 
%C2D to convert continuous actual to discrete actual 
%actual track of worker 



%circular motion of the server radius, Rs. 

% xO is the position vector [x,y] of the server 

% Set up circular path for server. Example is radius of 100 meteres at w=0.015 rad/sec. 



%System dynamic model of server measurement process for the range measurement 



As=[0,w;-w,0];q=[l;l]; 
x0=zeros(2,2000);x0(:, 1 )=[Rs;0] ; 

[p,g]=c2d(As,q,l); 
for i=l:(length(x0)-l); 
x0(:,i+l)=p*x0(:,i); 

R(i)=sqrt((x0(l,i)“xl(l,i))^2+(x0(2,i)>xl(2,i))^2); %Range between estimated worker position and 

%server vehicle; 

Ra(i)=sqrt((x0(l,i)-xla(l,i))^2+(x0(2,i)>xla(2,i))'^2); %actual Range between worker and server vehicles 
end 
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figure(l),clf,plot(xO(2,:),xO(l,:),'b-\xl(2,:),xl(l,;X’g.’),grid 
xlabel(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 
ylabel(\fontname { courier ne w }\fontsize { 1 2 }\bf METERS’); 
title(\fontname{ courier new)\fontsize{ 12}\bf AUV PATHS’); 
legend( Master AUV Path’, Desired Drone AUV Path’), axis equal 

figure(2),clf,plot(R,’g*’),grid 

hold;pIot(Ra,'b-’); 

xlabel(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 

ylabel(\fontname{ courier new}\fontsize{ 12 }\bf RANGE (METERS)’); 

title(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL VS DEAD RECKONING RANGE’); 

Iegend(Dead Reckoning Range’, ’Actual Range ’,0) 

figure(3),clf,pIot(x 1 (2,:),x 1 ( 1 ,:)/g-. ’,x 1 a(2,:),x la( 1 ,:),’r*’,x0(2,:),x0( 1 ,:),'b^’),grid,axis equal, 

xlabel(\fontname{courier new}\fontsize{ 1 2 }\bf METERS’); 

ylabel(\fontname{ courier new}\fontsize{ 1 2 )\bf METERS’); 

title(\fontname{ courier new}\fontsize{ 12)\bf AUV PATHS’); 

legend(Desired Drone AUV Path’, ’Actual Drone AUV Path’, Master AUV Path’,0) 

%build a Kalman filter for server vehicle 

% Position Estimator based on data stream, R, 

% 

endSample=length(Ra);startSample=l; 

y=[Ra;x0(l,l:length(Ra));x0(2,l:length(Ra));xla(l,l:length(Ra));xla(2,l:length(Ra));xla(3,l:length(Ra))]; 

x=zeros(5,endSample);err=zeros(6,endSample); 

s=startS ample; 

x(:,s)=[x0( 1 , 1 ),x0(2, 1 ),0,0,0] ’; 



^MANEUVERING AND CURRENT TIME CONSTANTS 



taul=0; 

tau2=0; 



A=[As,zeros(2,3);zeros(3,2),Aw]; 



C=zeros(6,5); %Initializes the C matrix 

C(2,1)=1;C(3,2)=1;C(4,3)=0;C(5,4)=0;C(6,5)=0; %Matches measurement states (y) to output states (x) 

%with drone states unobservable 

Ra( 1 )=sqrt((x0( 1 , 1 )-x 1 a( 1 , 1 ))'^2+(x0(2, 1 )-x 1 a(2, 1 ))^2); 

% Cmatrix local is dg/dx 

C( 1 ,3)=-(x0( 1 , 1 )-x 1 a( 1 , 1 ))/Ra( 1 ); 

C( 1 ,4)=-(x0(2, 1 )-x 1 a(2, 1 ))/Ra( 1 ); 

C(l,l)=(xO(l,l)-xla(l,l))/Ra(l); 

C( 1 ,2)=(x0(2, 1 )-x 1 a(2, 1 ))/Ra( 1 ); 



%Initial B matrix 

q 1=0.01; 

q2=0.01; 

q3=0.1; 

q4= 1; 

q5=0.0001; 



%variance on X, m^^2 
%variance on Y, m^^2 
% variance on xla 
% variance on yla, rad/s)'^2 
%slow bias convergence 
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B=[ql;q2;q3;q4;q5]/100; 

Q=diag(B); 

%system noise 

nu 1=0.01 ;nu2=.001;nu3=. 001 ;nu4=2000;nu5=2000;nu6=.02; 
gnu=[nul;nu2;nu3;nu4;nu5;nu6]; 

R=diag(gnu)* 1 ; %measurement noise 

Gram=zeros(5,5); %initialization of observability grammian matrix 

% measured data at old time, new_before means model predicted value 

p_old_after=eye(5) ; 
delx_old_after=zeros(5, 1 ); 

g=ones(6, 1 );psave=zeros(5,length(R)); 

dt=l;phi=expm(A*dt); t=0:l:endSample-l; 

for i=startSample:(endSample-l); 

%compute linearized PHI matrix using updated A 

%reset initial state 

x_old_after=x( : ,i) ; 

% nonlinear state propagation 

[ x_n e w_be f ore ] =phi *x_ol d_after ; 

%error covariance propagation 

p_new_before=phi*p_old_after*phi’+ Q; %new gain calculation using 

%Iinearized new C matrix and 
%current state error covariances. 



%formulate the innovation using nonlinear output propagation 
%as compared to new sampled data from measurements 



yhat=output3(x_new_before); 
err(:,i+l)=(y(:,i+l) - yhat); 

G=p_new_before*C’*inv(C*p_new_before*C+ R); % Kalman Gain 

p_old_after=[eye(5) - G*C]*p_new_before; 
psave(:,i+ 1 )=diag(p_old_after); 

cpc=inv(C*p_old_after*CVR); 

rel(i+l)=err(:,i+l)’*cpc*err(:,i+l); 

x_new_after=x_new_before + G*err(:,i+1); 
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%carry new state into next update 
x(:,i+l )=x_new_after; 



C=zeros(6,5); 

C(2, 1 )= 1 ;C(3,2)= 1 ;C(4,3)=0;C(5,4)=0;C(6,5)= 1 ; 

% Cmatrix local is dg/dx 

C(l,3)=-(x(l,i)-x(3,i))/y(l,i); 

C(l,4)=-(x(2,i)-x(4,i))/y(l,i); 

C(U)=-C(1,3); 

C(l,2)=-C(l,4); 

0(i)=rank([C’,phi’*Cl); 

U(i)=x(4,i)-y(5,i);T(i)=x(3,i)-y(4,i); 

diff(i)=sqrt(U(i)^2+T(ir2); 

Gram=Gram+phi ’*C ’*inv(R) *C *phi ; 

Gramrank(i)=rank(Gram); 

end; 

figure(4),clf,plot(0,’b*’),grid %Local Observability indicates # of observable states at a point in time. 
xIabel(\fontname{ courier new)\fontsize{ 12}\bf TIME (SEC)’); 
yIabeI(\fontname{courier new}\fontsize{ 12}\bf OBSERVABLE STATES’); 
titIe(\fontname{ courier new}\fontsize{ 12}\bf LOCAL SYSTEM OBSERVABILITY’) 

figure(5),clf,plot(Gramrank,’b*’),grid,axis([0 length(xO) 4 6]) ^Observability Grammian Rank indicates 

%of observable states over period of the 
%simulation 

xlabeI(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabeI(\fontname{ courier new}\fontsize{ 12}\bf OBSERVABLE STATES’); 
title(\fontname{ courier new}\fontsize{ 12}\bf TOTAL SYSTEM OBSERVABILITY’) 

figure(6),cIf,plot(t»reI,’b*’), grid, zoom 

figure(7),clf,plot(t,x(4,:),’b*’,t,xla(2,l: 1999), ’m.’), grid %East-West position comparison 

xlabel(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)*); 
ylabeI(\fontname{ courier new}\fontsize{ 12 }\bf METERS’); 

titIe(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL vs. ESTIMATED EAST-WEST POSITION’); 
Iegend( Estimated position’, ’Actual position’,0) 

figure(8),clf,plot(t,x(3,:),’b*’,t,xla( 1,1: 1999), ’m.’),grid %North-south position comparison 

xlabel(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabeI(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 

titIe(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION’); 
legend( Estimated position’, ’Actual position’,0) 

figure(9),clf,plot(x(2,:),x(l,:)), grid, hold, 

pIot(x(4,:),x(3,:),’r.’),plot(x 1 (2,:),x 1 (1 ,:),’m.’),plot(x la(2, 1 : 1 999),x 1 a( 1 , 1 : 1999),’c. ’);hold off,axis 
equaI,zoom 

xIabel(\fontname{courier new}\fontsize{ 1 2 }\bf METERS’); 
ylabeI(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 
title(\fontname{courier new}\fontsize{ 12}\bf AUV PATHS’); 

legend (Master AUV Path’, Estimated Drone Path’, Desired Drone AUV Path’, ’Actual Drone AUV Path’,0) 



84 



figure(10),clf,plot(err’),grid, 

xlabeI(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabeI(\fontname{ courier new }\fontsize{ 12 }\bf METERS’); 

tilIe(’Vfontname{ courier new}\fontsize{ 12}\bf DIFFERENCE BETWEEN ACTUAL AND ESTIMATED 
VALUES’); 

Iegend(’Range’, ’Server North-South’, ’Server East- West’, ’Worker North-South’, ’Worker East-West’, Worker 
Heading’, 0) 

figure(l l),cIf,plot(diff,’r-’),grid,hoId,pIot(DIFF,b.’),hoId off 
xIabel(\fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)’); 
ylabel(\fontname{ courier new}\fontsize{ 1 2 }\bf METERS’); 

titIe(\fontname{ courier new)\fontsize{ 1 2 }\bf NORMALIZED DIFFERENCES IN POSITION AT TIME 
(T)0; 

Iegend(’ROM Position Estimate’, Dead Reckoning’, 0) 
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APPENDIX C. MATLAB CODE ASSOCIATED WITH EXTENDED 
KALMAN FILTER MEASUREMENT DATA FUSION 



%Jason Alleyne 
%0utput3.m 

^Function required for ROM EBCF used in initpos.m and no*initpos.m 

function [yhat]=output3(xold); 

x0=xold(2); 

yO=xold(l); 

xl=xold(4); 

yl=xold(3); 

psil=xold(5); %ensure the states are in the same order as in main program 

Rhat=sqrt((xO-x 1 )^2+(y0-y 1 )'^2); 

xOhat=xO; 

yOhat=yO; 

y hat=[Rhat;yO;xO;y 1 ;x 1 ;psi 1 ] ; 
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